
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_land.c
  * @author     baiyang
  * @date       2022-3-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool mland_init(mode_base_t mode, bool ignore_checks);
static void mland_run(mode_base_t mode);
static void mland_gps_run(mode_base_t mode);
static void mland_nogps_run(mode_base_t mode);
static ModeNumber mland_mode_number(mode_base_t mode);
static bool mland_requires_GPS(mode_base_t mode);
static bool mland_has_manual_throttle(mode_base_t mode);
static bool mland_allows_arming(mode_base_t mode, enum ArmingMethod method);
static bool mland_is_autopilot(mode_base_t mode);
static bool mland_is_landing(mode_base_t mode);

static const char *mland_name(mode_base_t mode);
static const char *mland_name4(mode_base_t mode);
/*----------------------------------variable----------------------------------*/
static struct mode_ops mode_land_ops = {
        .mode_number = mland_mode_number,
        .init        = mland_init,
        .exit        = NULL,
        .run         = mland_run,
        .requires_GPS = mland_requires_GPS,
        .has_manual_throttle = mland_has_manual_throttle,
        .allows_arming = mland_allows_arming,
        .is_autopilot = mland_is_autopilot,
        .has_user_takeoff = NULL,
        .in_guided_mode = NULL,
        .logs_attitude = NULL,
        .allows_save_trim = NULL,
        .allows_autotune = NULL,
        .allows_flip = NULL,
        .name = mland_name,
        .name4 = mland_name4,
        .is_taking_off = NULL,
        .is_landing = mland_is_landing,
        .requires_terrain_failsafe = NULL,
        .get_wp = NULL,
        .wp_bearing = NULL,
        .wp_distance = NULL,
        .crosstrack_error = NULL,
        .output_to_motors = NULL,
        .use_pilot_yaw = NULL,
        .throttle_hover = NULL,
        .do_user_takeoff_start = NULL,
        .set_speed_xy = NULL,
        .set_speed_up = NULL,
        .set_speed_down = NULL,
        .pause = NULL,
        .resume = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/// ModeLoiter
void mode_land_ctor(ModeLand* mode_land)
{
    mode_ctor(&mode_land->mode, &mode_land_ops);
}

// land_init - initialise land controller
static bool mland_init(mode_base_t mode, bool ignore_checks)
{
    ModeLand* mode_land = (ModeLand*)mode;

    // check if we have GPS and decide which LAND we're going to do
    mode_land->control_position = fms_position_ok();
    if (mode_land->control_position) {
        // set target to stopping point
        Vector2f_t stopping_point;
        loiter_get_stopping_point_xy(mode->loiter_nav, &stopping_point);
        loiter_init_target2(mode->loiter_nav, &stopping_point);
    }

    // set vertical speed and acceleration limits
    posctrl_set_max_speed_accel_z(mode->pos_control, -fms_get_pilot_speed_dn(), fms.g.pilot_speed_up, fms.g.pilot_accel_z);
    posctrl_set_correction_speed_accel_z(mode->pos_control, -fms_get_pilot_speed_dn(), fms.g.pilot_speed_up, fms.g.pilot_accel_z);
    posctrl_set_max_speed_accel_xy(mode->pos_control, mode->loiter_nav->_speed_cms, mode->loiter_nav->_accel_cmss);
    
    // initialise the vertical position controller
    if (!posctrl_is_active_z(mode->pos_control)) {
        posctrl_init_z_controller(mode->pos_control);
    }

    mode_land->land_start_time = time_millis();
    mode_land->land_pause = false;

    // reset flag indicating if pilot has applied roll or pitch inputs during landing
    fms.ap.land_repo_active = false;

    // initialise yaw
    autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);

    return true;
}

// land_run - runs the land controller
// should be called at 100hz or more
void mland_run(mode_base_t mode)
{
    ModeLand* mode_land = (ModeLand*)mode;

    if (mode_land->control_position) {
        mland_gps_run(mode);
    } else {
        mland_nogps_run(mode);
    }
}

// land_gps_run - runs the land controller
//      horizontal position controlled with loiter controller
//      should be called at 100hz or more
static void mland_gps_run(mode_base_t mode)
{
    ModeLand* mode_land = (ModeLand*)mode;

    // disarm when the landing detector says we've landed
    if (fms.ap.land_complete && fms.motors->_spool_state == MOTOR_GROUND_IDLE) {
        mb_arming_disarm(ARMING_METHOD_LANDED, true);
    }

    // Land State Machine Determination
    if (mode_is_disarmed_or_landed()) {
        mode_make_safe_ground_handling(mode, false);
        loiter_clear_pilot_desired_acceleration(mode->loiter_nav);
        loiter_init_target(mode->loiter_nav);
    } else {
        // set motors to full range
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

        // pause before beginning land descent
        if (mode_land->land_pause && time_millis()-mode_land->land_start_time >= LAND_WITH_DELAY_MS) {
            mode_land->land_pause = false;
        }

        // run normal landing or precision landing (if enabled)

        mode_land_run_normal_or_precland(mode, mode_land->land_pause);
    }
}

// land_nogps_run - runs the land controller
//      pilot controls roll and pitch angles
//      should be called at 100hz or more
void mland_nogps_run(mode_base_t mode)
{
    ModeLand* mode_land = (ModeLand*)mode;

    float target_roll = 0.0f, target_pitch = 0.0f;
    float target_yaw_rate = 0;

    // process pilot inputs
    if (!fms.failsafe.radio) {
        if ((fms.g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && lpf_get_output(&fms.rc_throttle_control_in_filter) > LAND_CANCEL_TRIGGER_THR){
            //AP::logger().Write_Event(LogEvent::LAND_CANCELLED_BY_PILOT);
            // exit land if throttle is high
            fms_set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
        }

        if (fms.g.land_repositioning) {
            // apply SIMPLE mode transform to pilot inputs
            //update_simple_mode();

            // get pilot desired lean angles
            fms_get_pilot_desired_lean_angles(&target_roll, &target_pitch, fms.g.angle_max, attctrl_get_althold_lean_angle_max_cd(mode->attitude_control));
        }

        // get pilot's desired yaw rate
        target_yaw_rate = fms_get_pilot_desired_yaw_rate(rc_norm_input_dz(fms.channel_yaw));
        if (!math_flt_zero(target_yaw_rate)) {
            autoyaw_set_mode(mode->auto_yaw, AUTO_YAW_HOLD);
        }
    }

    // disarm when the landing detector says we've landed
    if (fms.ap.land_complete && mode->motors->_spool_state == MOTOR_GROUND_IDLE) {
        mb_arming_disarm(ARMING_METHOD_LANDED, true);
    }

    // Land State Machine Determination
    if (mode_is_disarmed_or_landed()) {
        mode_make_safe_ground_handling(mode, false);
    } else {
        // set motors to full range
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);

        // pause before beginning land descent
        if (mode_land->land_pause && time_millis()-mode_land->land_start_time >= LAND_WITH_DELAY_MS) {
            mode_land->land_pause = false;
        }

        mode_land_run_vertical_control(mode, mode_land->land_pause);
    }

    // call attitude controller
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(mode->attitude_control, target_roll, target_pitch, target_yaw_rate);
}

// do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
//  called during GPS failsafe to ensure that if we were already in LAND mode that we do not use the GPS
//  has no effect if we are not already in LAND mode
void mland_do_not_use_GPS(mode_base_t mode)
{
    ModeLand* mode_land = (ModeLand*)mode;
    mode_land->control_position = false;
}

static ModeNumber mland_mode_number(mode_base_t mode) { return LAND; }
static bool mland_requires_GPS(mode_base_t mode) { return false; }
static bool mland_has_manual_throttle(mode_base_t mode) { return false; }
static bool mland_allows_arming(mode_base_t mode, enum ArmingMethod method) { return false; };
static bool mland_is_autopilot(mode_base_t mode) { return true; }
static bool mland_is_landing(mode_base_t mode) { return true; };

static const char *mland_name(mode_base_t mode) { return "LAND"; }
static const char *mland_name4(mode_base_t mode) { return "LAND"; }

/*------------------------------------test------------------------------------*/


